#include "protocol.h"
#include "protocol_camera.h"

#define   	camera_group		(0)
#define		camera_slaveNodes	(4)
//FunType_CANTx		camera_lowTx;
FunType_bufferTx   camera_lowTx;
list_prot 	camera_queue_tx;
mesg_prot	camera_info[camera_slaveNodes];
ad_result	camera_result[camera_slaveNodes]; 

void queue_insert_camera(pmesg_prot pmesg)
{
	uint8_t pos, status, len, i;
	plist_prot pqueue;
	pqueue = &camera_queue_tx;
	pos = pqueue->head;
	if(pos>=mesg_max)pos = 0;
	status = pmesg->status;
	if(0==status){
		len = pmesg->len;
		pqueue->mesg_queue[pos].len = len;
		pqueue->mesg_queue[pos].status = status;
		pqueue->mesg_queue[pos].ID = pmesg->ID;
		for(i=0; i<len; i++){
			pqueue->mesg_queue[pos].buffer[i] = pmesg->buffer[i];
		}
		pqueue->head++;
		if(pqueue->head>=mesg_max)pqueue->head=0;
		if(pqueue->size<mesg_max)pqueue->size++;		
	}
}

static uint8_t camera_process_NST(uint8_t len, uint8_t data[], pmesg_prot mesg)
{
	uint8_t i;
	mesg_prot Tmesg;
	Tmesg.status = 0;
	Tmesg.ID = mesg->ID+0x80;
	for(i=0; i<len; i++){
		Tmesg.buffer[i] = data[i];
	}
	queue_insert_camera(&Tmesg);
	return 0;
}
static void camera_process_control(pmesg_prot mesg)
{
	uint8_t count;
	uint8_t data_Tx[8];
	count = 0;
	data_Tx[count++] = 0x10;
	data_Tx[count++] = 0xFF;
	camera_process_NST(count, data_Tx, mesg);
}
static void camera_process_status(pmesg_prot mesg)
{
	uint8_t count;
	uint8_t data_Tx[8];
	count = 0;
	data_Tx[count++] = 0x10;
	data_Tx[count++] = 0xFF;
	camera_process_NST(count, data_Tx, mesg);
}
static void camera_process_config(pmesg_prot mesg)
{
	uint8_t count;
	uint8_t data_Tx[8];
	count = 0;
	data_Tx[count++] = 0x10;
	data_Tx[count++] = 0xFF;
	camera_process_NST(count, data_Tx, mesg);
}
static void camera_process_resp(pmesg_prot mesg)
{
	uint16_t index ;
	if(0==mesg->buffer[6]){
		index = (mesg->ID)>>8;
		camera_result[index].sq_distance = mesg->buffer[7];
		camera_result[index].angle = mesg->buffer[8];
	}
}
static void camera_process_heartRx(pmesg_prot mesg)
{
	uint16_t index ;
	index = (mesg->ID)>>8;
	camera_result[index].workstatus = mesg->buffer[1];
	camera_result[index].runstatus = mesg->buffer[2];
}
static void camera_process_ErrorRx(pmesg_prot mesg)
{
	uint8_t count;
	uint8_t data_Tx[8];
	count = 0;
	data_Tx[count++] = 0x10;
	data_Tx[count++] = 0xFF;
	camera_process_NST(count, data_Tx, mesg);
}

uint8_t camera_process(pmesg_prot mesg)
{
	uint8_t count;
	uint8_t data_Tx[8];
	//len = mesg->len;
	switch(mesg->buffer[0]){
		case(0x10)://control
			camera_process_control(mesg);
		break;
		case(0x20)://status check
			camera_process_status(mesg);
		break;
		case(0x30)://
			camera_process_config(mesg);
		break;
		case(0x21):
			camera_process_resp(mesg);
		break;
		case(0x22):
			camera_process_heartRx(mesg);
		break;
		case(0x25):
			camera_process_ErrorRx(mesg);
		break;
		default:
			//main sid not support
			count = 0;
			data_Tx[count++] = 0xFF;
			camera_process_NST(count, data_Tx, mesg);
		break;
	}
	return 0;
}

uint8_t camera_rxFunc(void)
{
	return 0;	
}

static uint8_t camera_frametx(pmesg_prot mesg)
{
	uint8_t i, j, index, checksum, m, n;
	uint8_t buffer[12];
	uint8_t buffer_len = 0;
	uint32_t id;
	//checksum get
	checksum = 0;
	for(i=0; i<mesg->len; i++){
		checksum += mesg->buffer[i];
	}
	mesg->buffer[mesg->len] = checksum;
	//mesg->len++;//first byte
	mesg->len++;//checksum
	id = mesg->ID;
	m = mesg->len/8;
	n = mesg->len%8;
	index = 0;
	for(i=0; i<m; i++)
	{
		if(0==i){
			buffer[0] = mesg->len;
			for(j=1; j<8;j++){
				buffer[j] = mesg->buffer[index++];
			}
		}else{
			for(j=0; j<8;j++){
				buffer[j] = mesg->buffer[index++];
			}
		}
		buffer_len = 8;
		//id insert
		for(j=0; j<4; j++){
			buffer[buffer_len++] = (uint8_t)(id>>(j*8));
		}
		//tx frame
		camera_lowTx(buffer, buffer_len);
		id++;		
	}
	//for(i=0; i<n; i++)
	//{
		//len <8
		if(0==m){
			buffer[0] = mesg->len;
			for(j=0; j<n;j++){
				buffer[1+j] = mesg->buffer[index++];
			}
			//n = n+1;
			buffer_len = n + 1;
		}else{
			for(j=0; j<n;j++){
				buffer[j] = mesg->buffer[index++];
			}
			buffer_len = n;
		}
		//id insert
		for(j=0; j<4; j++){
			buffer[buffer_len++] = (uint8_t)(id>>(j*8));
		}
		//tx frame
		camera_lowTx(buffer, buffer_len);
		//id++;		
	//}
	return 0;
}
uint8_t camera_txFunc(void)
{
	uint8_t result;
	uint8_t pos;
	plist_prot pqueue;
	//second, rx data pro
	pqueue = &camera_queue_tx;
	//size = pqueue->size;
	if(pqueue->size>0){
	//while(pqueue->size>0){
		pos = pqueue->tail;
		if(pos>=mesg_max)pos=0;
		//result pro
		result = camera_frametx(&(pqueue->mesg_queue[pos]));
		pqueue->tail++;
		if(pqueue->tail>=mesg_max)pqueue->tail=0;		
		pqueue->size--;
	}
	return result;
}

void camera_rxFunc_callback(uint32_t id, uint8_t buffer[], uint8_t len)
{
	uint8_t i;
	uint16_t index ;
	static uint32_t id_last;
	if((id>0x100)&&(id<((camera_slaveNodes+1)<<8))){
		index = id>>8;
		if((index<<8)==id){
			id_last = id;
			camera_info[index].ID = id;
			camera_info[index].status = 1;
			camera_info[index].len = buffer[0];
			camera_info[index].index = 0;
			for(i=0; i<len; i++){
				camera_info[index].buffer[camera_info[index].index++] = buffer[i+1];
			}
		}else{
			if(1==camera_info[index].status){
				if((id_last+1)==id)
				{
					id_last = id;
					for(i=0; i<len; i++){
						camera_info[index].buffer[camera_info[index].index++] = buffer[i+1];
					}
				}else{
					//reset
					id_last = 0;
					camera_info[index].status = 0;
					camera_info[index].len = 0;
					camera_info[index].index = 0;
				}
			}
		}
		if(camera_info[index].index==camera_info[index].len){
			//insert data
			camera_info[index].status = 0;
			list_insert(&camera_info[index],camera_group);
			//reset
			id_last = 0;
			camera_info[index].status = 0;
			camera_info[index].len = 0;
			camera_info[index].index = 0;
		}
	}
}
/*
CMDS
*/
uint8_t camera_datamode_setting(uint8_t control, uint16_t period)
{
	uint8_t count;
	mesg_prot Tmesg;
	Tmesg.status = 0;
	Tmesg.ID = 0x100;
	count = 0;
	Tmesg.buffer[count++] = 0x10;
	Tmesg.buffer[count++] = 0x00;
	Tmesg.buffer[count++] = control;
	Tmesg.buffer[count++] = (uint8_t)(period>>0);
	Tmesg.buffer[count++] = (uint8_t)(period>>8);
	queue_insert_camera(&Tmesg);
	return 0;
}

uint8_t camera_workstatus_setting(uint8_t control)
{
	uint8_t count;
	mesg_prot Tmesg;
	Tmesg.status = 0;
	Tmesg.ID = 0x100;
	count = 0;
	Tmesg.buffer[count++] = 0x10;
	Tmesg.buffer[count++] = 0x01;
	Tmesg.buffer[count++] = control;
	queue_insert_camera(&Tmesg);
	return 0;
}

uint8_t camera_runstatus_setting(uint8_t control)
{
	uint8_t count;
	mesg_prot Tmesg;
	Tmesg.status = 0;
	Tmesg.ID = 0x100;
	count = 0;
	Tmesg.buffer[count++] = 0x10;
	Tmesg.buffer[count++] = 0x01;
	Tmesg.buffer[count++] = control;
	queue_insert_camera(&Tmesg);
	return 0;
}

uint8_t camera_runstatus_check(void)
{
	uint8_t count;
	mesg_prot Tmesg;
	Tmesg.status = 0;
	Tmesg.ID = 0x100;
	count = 0;
	Tmesg.buffer[count++] = 0x20;
	Tmesg.buffer[count++] = 0x00;
	Tmesg.len = count;
	queue_insert_camera(&Tmesg);
	return 0;
}

static uint8_t camera_lowtx(uint8_t *buffer, uint8_t len)
{
  return 0;  
}
/*
user can use this function to change low tx function
*/
void camera_lowTxfunc_Register(FunType_bufferTx txFunc)
{
	camera_lowTx = txFunc;
}

void camera_init(FunType_bufferTx txFunc)
{
	//register function
	func_register_init(camera_group, camera_rxFunc, camera_txFunc, camera_process);
	camera_lowTx = txFunc;
}

/*
this function should be call by application periodly
note: the speed set should be set by user condition
*/
uint8_t camera_task(void)
{
	uint8_t result = 0;
	//camera status get
	//camera_runstatus_check();
	result = list_check(camera_group);
	return result;
}
uint8_t camera_interfaceTest(void)
{
	uint8_t result = 0;
	//camera status get
	camera_runstatus_check();
	//result = list_check(camera_group);
	return result;
}




